#ifndef INTERFACE_H_
#define INTERFACE_H_

#include <iostream>
#include <sstream>
#include <map>
#include <math.h>
#include <string>


/*
 * Standard ros stuff
 */
#include <ros/ros.h>

//Image Messages
#include <sensor_msgs/Image.h>

//Image Transport
#include <image_transport/image_transport.h>
#include <camera_calibration_parsers/parse.h>

//Markers
#include <interactive_markers/interactive_marker_server.h>
#include <interactive_markers/menu_handler.h>

//Pose
#include <geometry_msgs/Pose.h>
#include <nav_msgs/Odometry.h>

//Sound
#include <sound_play/sound_play.h>


namespace seekDot {

class Interface
{
	//image_transport
	ros::NodeHandle &n;
	image_transport::ImageTransport it;


	//Subscribers
	image_transport::CameraSubscriber image_in;
	ros::Subscriber	pose_in;

	//Publishers
	image_transport::CameraPublisher image_display;
	image_transport::CameraPublisher image_save;

	//Marker Server
	boost::shared_ptr<interactive_markers::InteractiveMarkerServer> marker_server;

	//Menu Handler
	interactive_markers::MenuHandler menu_handler;

	//Menu Entry Handles
 	interactive_markers::MenuHandler::EntryHandle show_pic_handle;
	interactive_markers::MenuHandler::EntryHandle delete_pic_handle;
	interactive_markers::MenuHandler::EntryHandle save_pic_handle;

	//Last Position
	geometry_msgs::PoseWithCovariance last_pose;


	//Image Number
	unsigned int img_num;

	//Markerframe
	std::string marker_frame_id;


	//Sound
	boost::shared_ptr<sound_play::SoundClient> sndClient;

	//Class to store images with their camera_info
	class found
	{
	public:
		found(const sensor_msgs::Image &msg, const sensor_msgs::CameraInfo &camera_info, const geometry_msgs::PoseWithCovariance &pose)
		:msg(msg),camera_info(camera_info), pose(pose)
		{}
		~found(){}
		sensor_msgs::Image msg;
		sensor_msgs::CameraInfo camera_info;
		geometry_msgs::PoseWithCovariance pose;
	};

	std::map< std::string, found > found_imgs;

	public:
	Interface(ros::NodeHandle &n);
	virtual ~Interface();


	private:

	//Callback for processing pose information
	void processPose(const nav_msgs::Odometry::ConstPtr& msg);

	//Callback for processing images
	void processImage(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& camera_info);


	//Function to create Markers
	visualization_msgs::Marker makeDisc( visualization_msgs::InteractiveMarker &msg );
	visualization_msgs::InteractiveMarkerControl& makeBoxControl( visualization_msgs::InteractiveMarker &msg );
	visualization_msgs::InteractiveMarker makeEmptyMarker();

	//Menu Init for Picture Markers
	void initFoundMenu();

	//Menu Handler for Picture Markers
	void showFoundPic( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
	void deleteFoundPic( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
	void saveFoundPic( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );

	//Insert a Picture Marker
    void insertFoundMarker(const std::string& name,const geometry_msgs::Point& position);
};

}

#endif /* INTERFACE_H_ */
